﻿#include <learn/alg/alg_energy.h>

using namespace xi;
using namespace xi::alg;

double Alg_Energy::surface_least_square_energy(const std::vector<Vec3> &data, const std::vector<Vec2> &params,
                                               std::shared_ptr<Geom_Surface<3>> surface)
{
    return Alg_Distance<3>::distance<xi::alg::MeanSquareDistance>(data, params, surface);
}

std::tuple<double, double> Alg_Energy::surface_stretch_bending_energy(std::shared_ptr<Geom_BSplineSurface<3>> surface)
{
    auto [B1, B2] = Alg_CoeffMatrix::surface_stretch_bending(surface->u_knots(), surface->v_knots(),
                                                             surface->u_degree(), surface->v_degree());

    auto un = surface->u_pole_num();
    auto vn = surface->v_pole_num();
    auto N = un * vn;

    Eigen::MatrixXd P(N, 3);
    for (std::size_t i = 0; i < un; i++)
    {
        for (std::size_t j = 0; j < vn; j++)
        {
            auto p = surface->pole(i, j);
            P(i * vn + j, 0) = p.x();
            P(i * vn + j, 1) = p.y();
            P(i * vn + j, 2) = p.z();
        }
    }

    auto E1 = P.transpose() * B1 * P;
    auto E2 = P.transpose() * B2 * P;

    auto [U1, U2, V1, V2] = surface->bounds();
    double e1 = E1.trace() * (U2 - U1) * (V2 - V1);
    double e2 = E2.trace() * std::pow((U2 - U1) * (V2 - V1), 2);
    return std::make_tuple(e1, e2);
}